#include "can_uim.h"
#include <linux/can.h>
#include <string.h>
#include <stdio.h>
#include <list>
#include <iostream>
#include <unistd.h>
#include <pthread.h>

#include <sys/ipc.h>
#include <sys/types.h>
#include <sys/msg.h>

#include "msg_def.h"

#include "procedure.h"
#include "config.h"
#include "common.h"

#include "uim_motor_config.h"

using namespace std;

pthread_mutex_t mutex_uim_devs;
pthread_mutex_t mutex_uim_frames;

extern ob_manage * g_ptrObManage;

#ifdef _GP_1
uim_ctl_desc uimCtlArr[MAX_UIM_CTL_NUM] = {

    {
        .sid = 10,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 61679,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 2.4384,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 11,
        .ret_dir = RET_BACK_NEGITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 48000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 6.35,
        .loopType = OPEN_LOOP,

    },

    {
        .sid = 12,
        .ret_dir = NO_RET_BACK,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 10000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 0,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 13,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 22676,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 2.1167,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 14,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 110000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 0,
        .loopType = CLOSE_LOOP,
    },
    {
        .sid = 15,
        .ret_dir = RET_BACK_NEGITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 170000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 2.54,
        .loopType = CLOSE_LOOP,
    },
    {
        .sid = 16,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 15000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 71.12,
        .loopType = OPEN_LOOP,
    },

    {
        .sid = 17,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 5000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 0,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 18,
        .ret_dir = RET_BACK_NEGITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 4000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 0,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 19,
        .ret_dir = RET_BACK_NEGITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 5000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 20,
        .ret_dir = RET_BACK_NEGITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 150000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 21,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 150000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 22,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 80000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 23,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 900,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 24,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 900,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 25,
        .ret_dir = RET_BACK_NEGITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 900,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 26,
        .ret_dir = RET_BACK_NEGITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 900,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },
};

uim_ctl_desc uimCtlArr_pcr[MAX_UIM_CTL_NUM_PCR] = {

    {
        .sid = 40,
        .ret_dir = RET_BACK_NEGITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 32100,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 2.4384,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 41,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 25450,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 6.35,
        .loopType = OPEN_LOOP,

    },

    {
        .sid = 42,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 150000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 0,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 43,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 250000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 2.1167,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 44,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 150000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 0,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 45,
        .ret_dir = RET_BACK_NEGITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 35000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 2.54,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 46,
        .ret_dir = RET_BACK_NEGITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 35000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 71.12,
        .loopType = OPEN_LOOP,
    },

    {
        .sid = 47,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 150000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 0,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 48,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 11000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 0,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 49,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 150000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },

    {
        .sid = 50,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 150000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 51,
        .ret_dir = RET_BACK_NEGITIVE,
        .retFlag = STOP_WHEN_UP,
        .status = 0,
        .max_distance = 150000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 52,
        .ret_dir = RET_BACK_NEGITIVE,
        .retFlag = STOP_WHEN_DOWN,
        .status = 0,
        .max_distance = 35000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 0,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 53,
        .ret_dir = RET_BACK_NEGITIVE,
        .retFlag = STOP_WHEN_DOWN,
        .status = 0,
        .max_distance = 35000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },

    {
        .sid = 54,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_DOWN,
        .status = 0,
        .max_distance = 150000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },
    {
        .sid = 55,
        .ret_dir = RET_BACK_POSITIVE,
        .retFlag = STOP_WHEN_DOWN,
        .status = 0,
        .max_distance = 11000,
        .currPos = 0,
        .thetaPerStep = 1.8,
        .lead = 40,
        .loopType = OPEN_LOOP,
    },
};
#else
#ifdef _GP_BETA_2
uim_ctl_desc * uimCtlArr = uimCtlArr_beta2;
uim_ctl_desc * uimCtlArr_pcr = uimCtlArr_pcr_beta2;
#else
uim_ctl_desc * uimCtlArr = uimCtlArr_beta1;
uim_ctl_desc * uimCtlArr_pcr = uimCtlArr_pcr_beta1;
#endif
#endif

int parse_uim_data(void* ptrCanFrame,uim_data* dataOut)
{
    unsigned int canId;
    struct can_frame * canFrame =  (struct can_frame *)ptrCanFrame;

    if(ptrCanFrame==NULL || dataOut==NULL)
    {
        return -1;
    }
    canId = canFrame->can_id;
    dataOut->sid = ((canId&0x0300)>>3) + ((canId&0x00f80000)>>19);
    
    if(dataOut->sid < 5)
    {
        return -1;
    }

    dataOut->flag = 0x00;
    dataOut->tcw = canId&0xff;
    dataOut->dlc = (canFrame->can_dlc > 8)?8:canFrame->can_dlc;

   
    if(dataOut->dlc > 0)
    {
        memcpy(dataOut->data,canFrame->data,dataOut->dlc);
    }

    if(dataOut->sid & 0x40000)
    {
        dataOut->flag = 0x01;
    }
    else
    {
        dataOut->flag = 0x00;
    }
    

    return 0;
}

int encap_uim_data(uim_data * data,void* canFrameOut)
{
    struct can_frame * canFrame = (struct can_frame *)canFrameOut;

    if(canFrame==NULL || data==NULL)
    {
        return -1;
    }
    
    if((data->flag&0x10) == 0x10)
    {
        canFrame->can_id = (ACT_SID_P2G<<18) + ACT_EID_P2G(data->tcw);
    }
    else
    {
        canFrame->can_id = (ACT_SID_P2P(data->sid)<<18) + ACT_EID(data->sid,data->tcw);
    }
    canFrame->can_id &= CAN_EFF_MASK;
	canFrame->can_id |= CAN_EFF_FLAG;

    canFrame->can_dlc = (data->dlc > 8)?8:data->dlc;

    if(canFrame->can_dlc)
    {
        memcpy(canFrame->data,data->data,canFrame->can_dlc);
    }
    return 0;

} 

void print_uim_data(uim_data * dataToPrint)
{
   int i,n,n1;
   char strData[128];
   printf("recv uim data:\n");
   printf("node id:%d\n,tcw:ctlCode[%d],cmdCode[%d]\nFG:%d\ndlc:%d\n",
   dataToPrint->sid,
   ((dataToPrint->tcw&0x80)>>7),
   dataToPrint->tcw&0x7f,
   dataToPrint->flag&0x01,
   dataToPrint->dlc);
   if(dataToPrint->dlc)
   {
       n = snprintf(strData,128,"%02x ",dataToPrint->data[0]);
       for(i=1;i<dataToPrint->dlc;i++)
       {
           n1 = snprintf(strData+n,128-n,"%02x ",dataToPrint->data[i]);
           n += n1;
       }
       printf("%s\n",strData);
   }
}

void format_cmd(unsigned char ctl_code,unsigned char cmd_code,unsigned char sid,unsigned char dlc, unsigned char * data,uim_data *dataOut)
{
    unsigned char tempDlc;
    if(dataOut==NULL)
    {
        return;
    }
    dataOut->sid = sid;
    dataOut->tcw = cmd_code;
    dataOut->flag = 0x00;
    tempDlc = (dlc>8)?8:dlc;
    tempDlc = (tempDlc<0)?0:tempDlc;
    dataOut->dlc = tempDlc;

    if(tempDlc>0 && data!=NULL)
    {
        memcpy(dataOut->data,data,tempDlc);
    }    
    
}

CUim_MainCtl::CUim_MainCtl()
{
    if(!uimDevList.empty())
    {
        std::list<uim_dev>::iterator itListDev = uimDevList.begin();
        while(itListDev!=uimDevList.end())
        {
            if(!itListDev->frameList.empty())
            {
                itListDev->frameList.clear();
            }
            itListDev++;
        }
    }
    checkingRetList.clear();
    pTecanInstance = new CTecan();   

    uimDevList.clear();
}

CUim_MainCtl::~CUim_MainCtl()
{
    if(!uimDevList.empty())
    {
        std::list<uim_dev>::iterator itListDev = uimDevList.begin();
        while(itListDev!=uimDevList.end())
        {
            if(!itListDev->frameList.empty())
            {
                itListDev->frameList.clear();
            }
            itListDev++;
        }
    }
    uimDevList.clear();
    if(pTecanInstance)
    {
       pTecanInstance->setExitFlag(1); 
       delete pTecanInstance;
       pTecanInstance = NULL;
    }

    checkingRetList.clear();
    waitingRetList.clear(); 
}

int CUim_MainCtl::getDevStatus(unsigned int sid)
{
    int statusTmp = -1;
    if(uimDevList.empty())
    {
        return statusTmp;
    }
    list<uim_dev>::iterator itDev = uimDevList.begin();
    while(itDev!=uimDevList.end())
    {
        if(itDev->sid == sid)
        {
            statusTmp =  itDev->cur_status;
            break;
        }
       itDev++; 
    }
    return statusTmp;
}

int CUim_MainCtl::addDev(uim_dev* ptrDev)
{    
    if(isDevExist(ptrDev->sid))
    {
        printf("dev with sid[%d] already exists\n",ptrDev->sid);
        return -1;
    }
    uim_dev stDev;
    //memcpy(&stDev,ptrDev,sizeof(uim_dev));
    stDev.sid = ptrDev->sid;
    stDev.cur_status = ptrDev->cur_status;
    stDev.frameList.clear();
    printf("add dev with sid[%d]\n",stDev.sid);
    pthread_mutex_lock(&mutex_uim_devs);
    uimDevList.push_back(stDev);
    pthread_mutex_unlock(&mutex_uim_devs);

    this->initUimDev(stDev.sid);
    return 0;
}
//the sid must be set in dataOut
int CUim_MainCtl::getDevData(unsigned int sidIn,uim_frame_list & dataOut)
{
    int ret = -1;
    if(uimDevList.empty())
    {       
        return ret;
    }
    list<uim_dev>::iterator itDev = uimDevList.begin();
    while(itDev!=uimDevList.end())
    {
        if(itDev->sid == sidIn)
        {
            ret = 0;
            dataOut = itDev->frameList;
            break;
        }
       itDev++; 
    }
    return ret;

}

int CUim_MainCtl::setDevData(unsigned int sid,uim_frame_list & dataIn)
{
    int ret = -1;
    if(uimDevList.empty())
    {
        return ret;
    }
    list<uim_dev>::iterator itDev = uimDevList.begin();
    while(itDev!=uimDevList.end())
    {
        if(itDev->sid == sid)
        {
            ret = 0;
            itDev->frameList.clear();
            if(!dataIn.empty())
            {
               itDev->frameList.assign(dataIn.begin(),dataIn.end());
            }
            
            break;
        }
       itDev++; 
    }
    return ret;
}

int CUim_MainCtl::addData(uim_frame * dataIn)
{
    int ret = -1;

    if(dataIn == NULL)
    {
        return ret;
    }
    
    uim_frame tmpFrame;
    memcpy(&tmpFrame,dataIn,sizeof(uim_frame));
    list<uim_dev>::iterator itDev = uimDevList.begin();
    while(itDev!=uimDevList.end())
    {
        if(itDev->sid == dataIn->sid)
        {
            if(itDev->frameList.empty())
            {
                pthread_mutex_lock(&mutex_uim_frames);
                itDev->frameList.push_back(tmpFrame);
                pthread_mutex_unlock(&mutex_uim_frames);
            }
            else
            {
                /* code */
                uim_frame_list::iterator itUimFrame = itDev->frameList.begin();
                while(itUimFrame!=itDev->frameList.end())
                {
                    if(itUimFrame->tcw == dataIn->tcw)
                    {
                        if(itUimFrame->isValid && !itUimFrame->isComplete)
                        {//here is the consequent frame need to merge to the frame
                         // received more early
                            itUimFrame->isComplete = dataIn->isComplete;
                            
                            memcpy(itUimFrame->data+itUimFrame->len,
                                   dataIn->data,dataIn->len);
                            itUimFrame->len += dataIn->len;
                            break;
                        }
                    }
                    itUimFrame++;
                }
                 
                if(itUimFrame==itDev->frameList.end())
                {
                    pthread_mutex_lock(&mutex_uim_frames);
                    itDev->frameList.push_back(tmpFrame);
                    pthread_mutex_unlock(&mutex_uim_frames);
                }

            }
            break;
        }
       itDev++; 
    }

    if(uimDevList.empty() ||itDev == uimDevList.end())
    {

        if((dataIn->sid >=10 && dataIn->sid <(10+MAX_UIM_CTL_NUM))
        ||  (dataIn->sid >= 40 )&& dataIn->sid <(40+MAX_UIM_CTL_NUM_PCR))
        {
            uim_dev devToAdd;
            devToAdd.cur_status = 0;
            devToAdd.frameList.clear();
            devToAdd.sid = dataIn->sid;
            //devToAdd.frameList.push_back(tmpFrame);
            this->addDev(&devToAdd);
            list<uim_dev>::reverse_iterator itDev_r = uimDevList.rbegin(); 
            itDev_r->frameList.push_back(tmpFrame);
        }
    }
    return ret;
}


void CUim_MainCtl::uim_move_relative(void * paramIn)
{
    uim_data dataRet;
    struct can_frame frameToSend;
    uim_frame dataRead;
    int curStatus = NOT_START;

    move_param * stMoveParam = (move_param *)paramIn;

    printf("start move relative sid[%d],dir[%d]\n",stMoveParam->sid,stMoveParam->dir);

    //1,enable the H bridge
    format_cmd(1,ENA,stMoveParam->sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    //usleep(2000);

    //2, set the original point
    // unsigned char dataOrg[4] = {0x00,0x00,0x00,0x00};
    // format_cmd(1,ORG,sid,4, dataOrg,&dataRet);
    // encap_uim_data(&dataRet,&frameToSend);     
    // this->WriteCanPort(&frameToSend);

    //usleep(2000);

    //3,set the position desired to 0
    
    
    // if(loopType==CLOSE_LOOP)
    // {
    //     tmpPosCmd = QEC;
    // }
     unsigned char dataPos[4] = {0,0,0,0};
    // format_cmd(1,tmpPosCmd,sid,4, dataPos,&dataRet);
    // encap_uim_data(&dataRet,&frameToSend);     
    // this->WriteCanPort(&frameToSend);
    
    //usleep(2000);
    //4, set the speed
    unsigned int speed = stMoveParam->speed;
    unsigned char dataSpeed[4] = {speed&0x00ff,
    (speed>>8)&0x00ff,(speed>>16)&0x00ff,(speed>>24)&0x00ff};
    format_cmd(1,SPD,stMoveParam->sid,4, dataSpeed,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);



     //4.1, set the increase speed
    unsigned int inc_speed = stMoveParam->inc_speed;
    unsigned char dataIncSpeed[4] = {inc_speed&0x00ff,
    (inc_speed>>8)&0x00ff,(inc_speed>>16)&0x00ff,(inc_speed>>24)&0x00ff};
    format_cmd(1,MAC,stMoveParam->sid,4, dataIncSpeed,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    //4.2 set the decrease speed
    unsigned int dec_speed = stMoveParam->dec_speed;
    unsigned char dataDecSpeed[4] = {dec_speed&0x00ff,
    (dec_speed>>8)&0x00ff,(dec_speed>>16)&0x00ff,(dec_speed>>24)&0x00ff};
    format_cmd(1,MDE,stMoveParam->sid,4, dataDecSpeed,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);


    //5,set the position desired
    int tmpPos = (int)stMoveParam->pos;
    if(stMoveParam->dir == getELeCtlRetdir(stMoveParam->sid))
    {
        //this means go toward origin point
        tmpPos = 0 - tmpPos;
    }

    //unsigned char dataPos[4] = {tmpPos&0x00ff,
    //(tmpPos>>8)&0x00ff,(tmpPos>>16)&0x00ff,(tmpPos>>24)&0x00ff};
    dataPos[0] = tmpPos&0x00ff;
    dataPos[1] = (tmpPos>>8)&0x00ff;
    dataPos[2] = (tmpPos>>16)&0x00ff;
    dataPos[3] = (tmpPos>>24)&0x00ff;
    unsigned char tmpPosCmd = STP;
    format_cmd(1,tmpPosCmd,stMoveParam->sid,4, dataPos,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);

    setELeCtlStatus(stMoveParam->sid,MOVING);     
    this->WriteCanPort(&frameToSend);


    

    // this->setDevStatus(sid,INITIALIZED);
    // while(getDevStatus(sid)!= FINISHED)
    // {
    //     usleep(2000);
    //     dataRead.sid = sid;
    //     if(getDevData(&dataRead)==0)
    //     {
    //         if(dataRead.tcw == ENA)
    //         {   
    //             //2,set the original point;
    //             unsigned char dataOrg[4] = {0x00,0x00,0x00,0x00};
    //             format_cmd(1,ORG,sid,4, dataOrg,&dataRet);
    //             encap_uim_data(&dataRet,&frameToSend);     
    //             this->WriteCanPort(&frameToSend);
    //         }
    //         if(dataRead.tcw == ORG)
    //         {
    //             //3,set the position desired
    //             unsigned char dataPos[4] = {position&0x00ff,
    //             (position>>8)&0x00ff,(position>>16)&0x00ff,(position>>24)&0x00ff};
    //             format_cmd(1,POS,sid,4, dataPos,&dataRet);
    //             encap_uim_data(&dataRet,&frameToSend);     
    //             this->WriteCanPort(&frameToSend);
    //         }
    //         if(dataRead.tcw == POS)
    //         {
    //             //3,set the speed desired
    //             unsigned int speed = 1000;
    //             unsigned char dataPos[4] = {speed&0x00ff,
    //             (speed>>8)&0x00ff,(speed>>16)&0x00ff,(speed>>24)&0x00ff};
    //             format_cmd(1,SPD,sid,4, dataPos,&dataRet);
    //             encap_uim_data(&dataRet,&frameToSend);     
    //             this->WriteCanPort(&frameToSend);
    //             setDevStatus(sid,MOVING);
    //         }

    //         if(dataRead.tcw == SPD)
    //         {
    //             unsigned int currSpeed = 0;
    //             if(dataRead.len == 4)
    //             {
    //                 currSpeed = dataRead.data[0] + dataRead.data[1]<<8 + dataRead.data[2]<<16 + dataRead.data[3]<<24;
    //                 if(currSpeed == 0)
    //                 {
    //                     setDevStatus(sid,FINISHED);
    //                 }
    //             }

    //         }
    //     }

    //     /*if(getDevStatus(sid)==MOVING)
    //     {
    //         //send query frame to uim ele ctl device here, get the current speed.
    //         unsigned char dataPos[4] = {0xff,0xff,0xff,0xff};
    //         format_cmd(1,SPD,sid,4, dataPos,&dataRet);
    //         encap_uim_data(&dataRet,&frameToSend);     
    //         this->WriteCanPort(&frameToSend);
    //         setDevStatus(sid,MOVING);
    //     }*/

    // }

    //sleep(2);
    //diable the H bridge
    //format_cmd(1,OFF,sid,0, NULL,&dataRet);
    //encap_uim_data(&dataRet,&frameToSend);

    //this->WriteCanPort(&frameToSend);



    // int cntLoop = 0;
    // uim_frame_list frameList;
    // int flag = 0;
    // while(1)
    // {
    //     usleep(2000);
    //     if(getDevData(sid,frameList)==0)
    //     {
    //         if(!frameList.empty())
    //         {
    //             uim_frame_list::iterator itFrame = frameList.begin();
    //             while(itFrame!=frameList.end())
    //             {
    //                 if(itFrame->tcw == NOTIFY_MOVE_END)
    //                 {
    //                     int posEnd = 0;
    //                     unsigned char * tmpPtr = (unsigned char *)&posEnd;
    //                     if(itFrame->len > 0 && itFrame->len <= 4)
    //                     {
    //                         memcpy(tmpPtr,itFrame->data,itFrame->len);
    //                         printf("posEnd is %08x, %d\n",posEnd,posEnd);
    //                     } 
    //                     flag = 1;
    //                     pthread_mutex_lock(&mutex_uim_frames);
    //                     frameList.erase(itFrame);
    //                     pthread_mutex_unlock(&mutex_uim_frames);                      
    //                     break;               
    //                 }

    //                 itFrame++;
    //             }
    //         }
            
    //     }
    //     if(flag)
    //     {
    //         break;
    //     }
    //     cntLoop++;
    //     if(2000<cntLoop)
    //     {
    //         break;
    //     }
    // }


    // this->setDevStatus(sid,NOT_START);
    // int retRead = 0;
    // while(retRead==0)
    // {
    //    retRead = getDevData(&dataRead);
    //    printf("data in list,sid[%d],rcw[%d]\n",dataRead.sid,dataRead.tcw);
    // }
}

void CUim_MainCtl::uim_move(void * paramIn)
{
    uim_data dataRet;
    struct can_frame frameToSend;
    uim_frame dataRead;
    int curStatus = NOT_START;
    move_param * stMoveParam = (move_param *)paramIn;
    unsigned char sid = stMoveParam->sid;
    unsigned char direct =  stMoveParam->dir;    

    int valid = checkMoveParam_absolutePos(stMoveParam);

    direct =  stMoveParam->dir;

    if(!valid)
    {
        sendAckToGrpc(1,PARAM_INVALID,sid,-1);
        printf("move param is not valid\n");       
        return ;
    }

    if(stMoveParam->pos==0)
    {
        
        
        recvDataEleCtl stDataRecv;
        stDataRecv.sid = sid;
        stDataRecv.frameType = CHECK_MOVE_END;
        if(g_ptrObManage!=NULL )
        {
            //need param when update status.    
            
            //stDataRecv.frameType = rcw;
            if(g_ptrObManage)
            {
                g_ptrObManage->update(&stDataRecv);
            }           

            
        }

        
        if(getELeCtlAckFlag(sid) == MSG_FROM_LUA)
        {
            sendAckToLua(&stDataRecv);
        }
        else
        {
            sendAckToGrpc(0,0,sid,-1);
        }
        
        

        
        
        printf("no need to move, already on point\n");
        return ;
    }
    printf("start move sid[%d],dir[%d]\n",sid,direct);
    
    //1,enable the H bridge
    format_cmd(1,ENA,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    
    if(direct != getELeCtlRetdir(sid))//(sid==52 || sid==53 || sid==13 || sid==45 || sid==46)
    {//leave origin point
        //disable the rising and falling event
        // unsigned char s1con[3] = {0x11,0x00,0x00};
        // format_cmd(1,SCF,sid,3, s1con,&dataRet);
        // encap_uim_data(&dataRet,&frameToSend);     
        // this->WriteCanPort(&frameToSend);
        setSensorAction(sid,0);
    }

    //2.set the pos to 0
    unsigned char tmpPosCmd = STP;
    // if(loopType==CLOSE_LOOP)
    // {
    //     tmpPosCmd = QEC;
    // }
     unsigned char dataPos[4] = {0,0,0,0};
    //format_cmd(1,tmpPosCmd,sid,4, dataPos,&dataRet);
    //encap_uim_data(&dataRet,&frameToSend);     
    //this->WriteCanPort(&frameToSend);
    
    //usleep(2000);

    //5,set the position desired
    int tmpPos = (int)stMoveParam->pos;
    if(direct == 0)
    {
        tmpPos = 0 - tmpPos;
    }
    tmpPos += getELeCtlCurrPos(sid);    

    // if(tmpPos==-4000 || tmpPos==4000 || tmpPos==-2000)
    // {
    //     tmpPos += 100; 
    // }
    if(getELeCtlRetdir(sid) == NO_RET_BACK)
    {
            format_cmd(1,ORG,sid,4, dataPos,&dataRet);
            encap_uim_data(&dataRet,&frameToSend);     
            this->WriteCanPort(&frameToSend);

        tmpPos = (int)stMoveParam->pos;
        tmpPosCmd = STP;
    }
    else
    {
        /* code */
        if(getELeCtlLoopType(sid) == CLOSE_LOOP)
        {
           tmpPosCmd = QEC; 
        }
        else
        {
           tmpPosCmd = POS;
            /* code */
        }
        
        
    }
    

    //unsigned char dataPos[4] = {tmpPos&0x00ff,
    //(tmpPos>>8)&0x00ff,(tmpPos>>16)&0x00ff,(tmpPos>>24)&0x00ff};
    dataPos[0] = tmpPos&0x00ff;
    dataPos[1] = (tmpPos>>8)&0x00ff;
    dataPos[2] = (tmpPos>>16)&0x00ff;
    dataPos[3] = (tmpPos>>24)&0x00ff;
    printf("set postion to %d\n",tmpPos);

    
    format_cmd(1,tmpPosCmd,sid,4, dataPos,&dataRet);//STP
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    
    setELeCtlStatus(sid,MOVING);
     //3, set the speed
    unsigned int speed = stMoveParam->speed;
    if(direct == 0)
    {
       speed = 0 - speed;
    }
    //speed = 0 - speed;
    printf("set speed to %d\n",speed);
    unsigned char dataSpeed[4] = {speed&0x00ff,
    (speed>>8)&0x00ff,(speed>>16)&0x00ff,(speed>>24)&0x00ff};
    format_cmd(1,SPD,sid,4, dataSpeed,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    //4.1, set the increase speed
    unsigned int inc_speed = stMoveParam->inc_speed;
    unsigned char dataIncSpeed[4] = {inc_speed&0x00ff,
    (inc_speed>>8)&0x00ff,(inc_speed>>16)&0x00ff,(inc_speed>>24)&0x00ff};
    format_cmd(1,MAC,sid,4, dataIncSpeed,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    //4.2 set the decrease speed
    unsigned int dec_speed = stMoveParam->dec_speed;
    unsigned char dataDecSpeed[4] = {dec_speed&0x00ff,
    (dec_speed>>8)&0x00ff,(dec_speed>>16)&0x00ff,(dec_speed>>24)&0x00ff};
    format_cmd(1,MDE,sid,4, dataDecSpeed,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    
    
    if(getELeCtlRetdir(sid) != NO_RET_BACK)
    {
        int relativePos = stMoveParam->pos - getELeCtlCurrPos(sid);
        relativePos = relativePos>0?relativePos:0-relativePos;

        int abSpeed = stMoveParam->speed;

        int timeout_ms = (stMoveParam->speed*1000/stMoveParam->inc_speed) + 
        (stMoveParam->speed*1000/stMoveParam->dec_speed) + (relativePos*1000/stMoveParam->speed);
        
        int totalWaitTime = 0;
        int queryTimes = 0;
        int currStatus = getELeCtlStatus(sid);

        if(getELeCtlLoopType(sid) == CLOSE_LOOP)
        {
            timeout_ms = ((timeout_ms*5)>>2);
        }
        timeout_ms += 2000;
        printf("timeout_ms:%d,currStatus:%d\n",timeout_ms,currStatus);
        while(currStatus == MOVING ||  currStatus == RECV_POS)
        {
           usleep(200000);
           totalWaitTime += 200000;
           currStatus = getELeCtlStatus(sid);
           if(currStatus != MOVING &&  currStatus != RECV_POS)
           {
               break;
           }
           if(totalWaitTime>(timeout_ms*1000))
           {
                //unsigned char queryCode = 
                format_cmd(1,tmpPosCmd,sid,0, NULL,&dataRet);
                encap_uim_data(&dataRet,&frameToSend);
                    
                this->WriteCanPort(&frameToSend);

                queryTimes++;
                printf("query pos here,queryTimes[%d]\n",queryTimes);
                if(currStatus != RECV_POS)
                {
                    //move timeout here;
                    if(queryTimes>3)
                    { 
                        usleep(200000);
                        
                        tmpPos += 3*(queryTimes-5);
                        dataPos[0] = tmpPos&0x00ff;
                        dataPos[1] = (tmpPos>>8)&0x00ff;
                        dataPos[2] = (tmpPos>>16)&0x00ff;
                        dataPos[3] = (tmpPos>>24)&0x00ff;
                        format_cmd(1,tmpPosCmd,sid, 4, dataPos,&dataRet);
                        encap_uim_data(&dataRet,&frameToSend);
                        this->WriteCanPort(&frameToSend);
                                            
                        
                    }
                    
                }
                
                    //move timeout here;
                if(queryTimes>20)
                {
                    break;
                }
                    
                
                usleep(800000);
           }
           
           relativePos = getELeCtlCurrPos(sid) - tmpPos;
           relativePos = relativePos>0?relativePos:0-relativePos;
           currStatus = getELeCtlStatus(sid);
           if(currStatus == RECV_POS && relativePos < 5) 
           {
               //move end here;                
                sendAckToGrpc(0,0,sid,-1);


                if(g_ptrObManage!=NULL )
                {
                    //need param when update status.    
                    recvDataEleCtl stDataRecv;
                    stDataRecv.sid = sid;
                    stDataRecv.frameType = CHECK_MOVE_END;
                    //stDataRecv.frameType = rcw;
                    g_ptrObManage->update(&stDataRecv);
                }
              break;  
           }
           else if(currStatus == RECV_POS)
           {
               printf("tmpPos:%d,stMoveParam.pos:%d,currPos:%d\n",
               tmpPos,stMoveParam->pos,getELeCtlCurrPos(sid));
           }           
           
        }

    }
        
    
}

void CUim_MainCtl::uim_stop(unsigned int sid)
{
    uim_data dataRet;
    struct can_frame frameToSend;
    uim_frame dataRead;
    
    unsigned char tmpPosCmd = STP;
    unsigned char dataPos[4] = {0,0,0,0};

    
    format_cmd(1,tmpPosCmd,sid,4, dataPos,&dataRet);//STP
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    tmpPosCmd = SPD;
    format_cmd(1,tmpPosCmd,sid,4, dataPos,&dataRet);//STP
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);


}

void CUim_MainCtl::setDevStatus(unsigned int sid, int status)
{
    if(uimDevList.empty())
    {
        return;
    }
    list<uim_dev>::iterator itDev = uimDevList.begin();
    while(itDev!=uimDevList.end())
    {
        if(itDev->sid == sid)
        {
            itDev->cur_status = status;
            break;
        }
       itDev++; 
    }
    return;
}


pthread_mutex_t mutex_param = PTHREAD_MUTEX_INITIALIZER;
extern int modify_param_xml(int fileId,const char* pathInXml,void * paramIn);

void CUim_MainCtl::cmdAct(void * param)
{
    ctl_uim * pstCtlUim = (ctl_uim *)param;
    int tempPostion = pstCtlUim->un_cmd.moveCtl.position;

    if(pstCtlUim->sid == 1)
    {
        //tecanActParam tecanCtlData
        printf("in CUim_MainCtl::cmdAct, to proc for tecan\n");
        if(pTecanInstance)
        {
            pTecanInstance->cmdAct(param,this);
        }        
        return;
    }
#ifdef  _QPCR
    if((pstCtlUim->sid < 40) 
        || (pstCtlUim->sid >= (40+MAX_UIM_CTL_NUM_PCR))
      )
#else
    if((pstCtlUim->sid < 10) 
        || (pstCtlUim->sid >= (10+MAX_UIM_CTL_NUM))
      )
#endif
    {
        printf("electl of sid[%d] is not on this board\n");
        return;

    }

    actThreadParam * pstActThreadParam = NULL;
    move_param *pstMoveParam = NULL;
    
    
    if(pstCtlUim->opcode == MOVE && tempPostion>0)
    {
        //
        printf("move cmd received,sid[%d],dir[%d],position[%d]\n",
        pstCtlUim->sid,pstCtlUim->un_cmd.moveCtl.dir,pstCtlUim->un_cmd.moveCtl.position);
        
        pthread_mutex_lock(&mutex_param);
        //int loopType = (pstCtlUim->sid&0x80)?CLOSE_LOOP:OPEN_LOOP;
        pstMoveParam = new move_param;
        //printf("new move param111,%p\n",pstMoveParam);
        memset(pstMoveParam,0,sizeof(move_param));
        pstMoveParam->dir = pstCtlUim->un_cmd.moveCtl.dir;
        pstMoveParam->dec_speed = (pstCtlUim->un_cmd.moveCtl.dec_speed == 0)?1000:pstCtlUim->un_cmd.moveCtl.dec_speed;
        pstMoveParam->pos = tempPostion;
        pstMoveParam->inc_speed = (pstCtlUim->un_cmd.moveCtl.inc_speed == 0)?1000:pstCtlUim->un_cmd.moveCtl.inc_speed;
        pstMoveParam->sid = pstCtlUim->sid&0x7f;
        pstMoveParam->speed = (pstCtlUim->un_cmd.moveCtl.speed == 0)?1000:pstCtlUim->un_cmd.moveCtl.speed;
        //this->uim_move(pstCtlUim->sid&0x7f,moveParam.dir,moveParam.pos,loopType);        
        pstActThreadParam = new actThreadParam;
        pstActThreadParam->actParam = pstMoveParam;
        pstActThreadParam->opcode = MOVE;
        pstActThreadParam->pCanInterface = this;

        pthread_mutex_unlock(&mutex_param);

        //this->uim_move(moveParam);
    }
    else if((pstCtlUim->opcode == RET_BACK) ||
           (tempPostion==0 && pstCtlUim->opcode == MOVE) )
    {
        //
        if(getELeCtlRetdir(pstCtlUim->sid) == NO_RET_BACK)
        {
           printf("no ret back for this sid[%d]\n",pstCtlUim->sid); 
           return;
        }
        
        pthread_mutex_lock(&mutex_param);
        printf("ret back cmd received, sid[%d]\n",pstCtlUim->sid);
        //int loopType = (pstCtlUim->sid&0x80)?CLOSE_LOOP:OPEN_LOOP;
        pstMoveParam = new move_param;
        printf("new retback param,%p\n",pstMoveParam);
        memset(pstMoveParam,0,sizeof(move_param));
        pstMoveParam->dir = pstCtlUim->un_cmd.moveCtl.dir;
        pstMoveParam->dec_speed = (pstCtlUim->un_cmd.moveCtl.dec_speed == 0)?1000:pstCtlUim->un_cmd.moveCtl.dec_speed;
        pstMoveParam->pos = pstCtlUim->un_cmd.moveCtl.position;
        pstMoveParam->inc_speed = (pstCtlUim->un_cmd.moveCtl.inc_speed == 0)?1000:pstCtlUim->un_cmd.moveCtl.inc_speed;
        pstMoveParam->sid = pstCtlUim->sid&0x7f;
        pstMoveParam->speed = (pstCtlUim->un_cmd.moveCtl.speed == 0)?1000:pstCtlUim->un_cmd.moveCtl.speed;
        //this->uim_ret_back(&moveParam);
        
        pstActThreadParam = new actThreadParam;
        pstActThreadParam->actParam = pstMoveParam;
        pstActThreadParam->opcode = RET_BACK;
        pstActThreadParam->pCanInterface = this;
        printf("set opcode to %d\n",pstActThreadParam->opcode);
        printf("speed:%d,inc_speed:%d,dec_speed:%d\n",
        pstMoveParam->speed,
        pstMoveParam->inc_speed,
        pstMoveParam->dec_speed);

        pthread_mutex_unlock(&mutex_param);
        
        
    }
    else if(pstCtlUim->opcode == STOP)
    {
        printf("stop cmd received,sid:%d\n",pstCtlUim->sid);
        this->uim_stop(pstCtlUim->sid);
    }
    else if(MOVE_RELATIVE == pstCtlUim->opcode)
    {
        printf("move relative received,sid:%d\n",pstCtlUim->sid);
        if(pstCtlUim->un_cmd.moveCtl.position > 0)
        {
            pthread_mutex_lock(&mutex_param);
                //printf("ret back cmd received, sid[%d]\n",pstCtlUim->sid);
                //int loopType = (pstCtlUim->sid&0x80)?CLOSE_LOOP:OPEN_LOOP;
                pstMoveParam = new move_param;
                printf("new move relative param,%p\n",pstMoveParam);
                memset(pstMoveParam,0,sizeof(move_param));
                pstMoveParam->dir = pstCtlUim->un_cmd.moveCtl.dir;
                pstMoveParam->dec_speed = (pstCtlUim->un_cmd.moveCtl.dec_speed == 0)?1000:pstCtlUim->un_cmd.moveCtl.dec_speed;
                pstMoveParam->pos = pstCtlUim->un_cmd.moveCtl.position;
                pstMoveParam->inc_speed = (pstCtlUim->un_cmd.moveCtl.inc_speed == 0)?1000:pstCtlUim->un_cmd.moveCtl.inc_speed;
                pstMoveParam->sid = pstCtlUim->sid&0x7f;
                pstMoveParam->speed = (pstCtlUim->un_cmd.moveCtl.speed == 0)?1000:pstCtlUim->un_cmd.moveCtl.speed;
                //this->uim_ret_back(&moveParam);
                
                pstActThreadParam = new actThreadParam;
                pstActThreadParam->actParam = pstMoveParam;
                pstActThreadParam->opcode = MOVE_RELATIVE;
                pstActThreadParam->pCanInterface = this;
                printf("set opcode to %d\n",pstActThreadParam->opcode);
                printf("speed:%d,inc_speed:%d,dec_speed:%d,dir:%d\n",
                pstMoveParam->speed,
                pstMoveParam->inc_speed,
                pstMoveParam->dec_speed,
                pstMoveParam->dir);

            pthread_mutex_unlock(&mutex_param);
        }
    }

    else if(UIM_MOTOR_INIT == pstCtlUim->opcode)
    {
        printf("init cmd received,sid:%d\n",pstCtlUim->sid);

        pthread_mutex_lock(&mutex_param);
        //printf("ret back cmd received, sid[%d]\n",pstCtlUim->sid);
        //int loopType = (pstCtlUim->sid&0x80)?CLOSE_LOOP:OPEN_LOOP;
        pstMoveParam = new move_param;
        memset(pstMoveParam,0,sizeof(move_param));
        pstMoveParam->sid = pstCtlUim->sid&0x7f;
        //this->uim_ret_back(&moveParam);
        
        pstActThreadParam = new actThreadParam;
        pstActThreadParam->actParam = pstMoveParam;
        pstActThreadParam->opcode = UIM_MOTOR_INIT;
        pstActThreadParam->pCanInterface = this;

        pthread_mutex_unlock(&mutex_param);

    }
    else if(UIM_GOZERO == pstCtlUim->opcode)
    {
        printf("UIM_GOZERO cmd received,sid:%d\n",pstCtlUim->sid);
        pthread_mutex_lock(&mutex_param);
        //printf("ret back cmd received, sid[%d]\n",pstCtlUim->sid);
        //int loopType = (pstCtlUim->sid&0x80)?CLOSE_LOOP:OPEN_LOOP;
        pstMoveParam = new move_param;
        memset(pstMoveParam,0,sizeof(move_param));
        pstMoveParam->sid = pstCtlUim->sid&0x7f;
        //this->uim_ret_back(&moveParam);
        
        pstActThreadParam = new actThreadParam;
        pstActThreadParam->actParam = pstMoveParam;
        if(getELeCtlStatus(pstMoveParam->sid)==NOT_START)
        {
            pstActThreadParam->opcode = UIM_MOTOR_INIT;  
        }
        else
        {
            pstActThreadParam->opcode = UIM_GOZERO;
        }
        
        pstActThreadParam->pCanInterface = this;

        pthread_mutex_unlock(&mutex_param);
    }


    printf("in cmdAct, pstCtlUim.opcode=%d\n",pstCtlUim->opcode);
    if(   pstCtlUim->opcode == MOVE || pstCtlUim->opcode == RET_BACK 
       || MOVE_RELATIVE == pstCtlUim->opcode 
       || UIM_MOTOR_INIT == pstCtlUim->opcode
       || UIM_GOZERO == pstCtlUim->opcode)
    {
        if(pstActThreadParam)
        {
            pthread_t currThread;
            setELeCtlAckFlag(pstCtlUim->sid,pstCtlUim->msgSrc);
            int ret = pthread_create(&currThread,NULL,uimActThreadFunc,(void*)pstActThreadParam);
            if(ret!=0)
            {
                printf("create thread for ret back failed\n");
            }

        }        
    }
    else
    {
        if(pstMoveParam)
        {
            delete pstMoveParam;
            pstMoveParam = NULL;
        }

        if(pstActThreadParam)
        {
            delete pstActThreadParam;
            pstActThreadParam = NULL;
        }
    }
}

int CUim_MainCtl::PackagePro( void * frame )
{

    uim_data recv_uim_data;
    struct can_frame * tmpFrame = (struct can_frame *)frame;
    int parseRet = 0;
    if(CAN_EFF_FLAG&(tmpFrame->can_id)) //extented frame
    {
        parseRet = parse_uim_data((void*)tmpFrame,&recv_uim_data);
        if(parseRet!=0)
        {
            printf("data is not from uim\n");
           return -1;
        }
    }
    else
    {

        if((tmpFrame->can_id & 0xfff) == SID_TECAN)
        {
            if(pTecanInstance)
            {
                pTecanInstance->addToBuff(tmpFrame->data,tmpFrame->can_dlc);
            }            
            return 0;
        }
        else
        {
            printf("received can data unexpected on uim can interface\n");
            printf("sid:%d,len:%d,",tmpFrame->can_id,tmpFrame->can_dlc);
            for(int i=0;i<tmpFrame->can_dlc;i++)
            {
                printf("%02x,",tmpFrame->data[i]);
            }
            printf("\n");
            return -1;
        }       
        
    }

    uim_data * dataRecv = &recv_uim_data;
    uim_frame frameToAdd;
    int tmpLen = 0;

    print_uim_data(dataRecv);
    
    if(dataRecv->tcw == gREG)
    {
        uim_dev oneDev;
        oneDev.sid = dataRecv->sid;
        oneDev.cur_status = NOT_START;
        this->addDev(&oneDev);
    }
    else
    {
        /* code */
        frameToAdd.sid = dataRecv->sid;
        frameToAdd.isComplete = dataRecv->flag&0x0f?0:1;
        frameToAdd.isValid = 1;
        frameToAdd.len = dataRecv->dlc;
        tmpLen = frameToAdd.len > 8?8:frameToAdd.len;
        if(tmpLen>0)
        {
            memcpy(frameToAdd.data,dataRecv->data,tmpLen);
        }
        frameToAdd.tcw = dataRecv->tcw;

        this->addData(&frameToAdd);

        uim_dev devFound;
        int retFind = this->getDev(dataRecv->sid,devFound);
        
        if(retFind)
        {
            printf("get dev with sid[%d] failed\n",dataRecv->sid);
            return -1;
        }
        uim_frame_list & listFrame = devFound.frameList;
        //this->getDevData(dataRecv->sid,listFrame);
        if(!listFrame.empty())
        {
           uim_frame_list::iterator itFrame = listFrame.begin();
           uim_frame_list::iterator itFrameTmp =  itFrame;
           while(itFrame!=listFrame.end())
           {
               if(itFrame->isComplete)
               {
                   ack_frame_proc(&(*itFrame));
                   itFrameTmp = itFrame;
                   itFrame++;
                   pthread_mutex_lock(&mutex_uim_frames);
                   listFrame.erase(itFrameTmp);
                   pthread_mutex_unlock(&mutex_uim_frames);
                   printf("element num in list,%d\n",listFrame.size());
                   continue;
               }
               itFrame++;
           }
           //the result put back
           this->setDevData(dataRecv->sid,listFrame);
           //printf("in dev, frame count:%d\n",devFound.frameList.size());
           //devFound.frameList = listFrame;
        }
        else
        {
            printf("listFrame in dev with sid[%d] is empty\n",dataRecv->sid);
        }
        
         
               
    }    
}

void CUim_MainCtl::findDevsInNet(void)
{
    //send the P2G frame out 
    uim_data dataRet;
    struct can_frame frameToSend;
    format_cmd(0,gREG,0,0, NULL,&dataRet);
    dataRet.flag = 0x10;//P2G
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    usleep(2000);
    
}

void CUim_MainCtl::clearData(void)
{
    CCan::clearData();

    if(!uimDevList.empty())
    {
        std::list<uim_dev>::iterator itListDev = uimDevList.begin();
        while(itListDev!=uimDevList.end())
        {
            if(!itListDev->frameList.empty())
            {
                itListDev->frameList.clear();
            }
            itListDev++;
        }
    }
    uimDevList.clear();
}


int CUim_MainCtl::isDevExist(unsigned int sid)
{
    int ret = 0;
    if(uimDevList.empty())
    {
        return ret;
    }
    list<uim_dev>::iterator itDev = uimDevList.begin();
    while(itDev!=uimDevList.end())
    {
       if(itDev->sid == sid)
       {
           ret = 1;
           break;
       }
       itDev++; 
    }

    return ret;
}

int CUim_MainCtl::initUimDev(unsigned int sid)
{
    uim_data dataRet;
    struct can_frame frameToSend;
    uim_frame dataRead;


    //format_cmd(1,MCF,sid,0, NULL,&dataRet);
   // encap_uim_data(&dataRet,&frameToSend);     
    //this->WriteCanPort(&frameToSend);


}

extern FILE * g_fp_z;
extern pthread_mutex_t mutexWriteDp8Z;

void CUim_MainCtl::ack_frame_proc(uim_frame * frameAck)
{
    unsigned int rcw = frameAck->tcw;
    unsigned short tmpShort = 0;
    unsigned ctlCode = (rcw&0x80)?1:0;
    
    rcw = rcw & 0x7f;

    recvDataEleCtl stDataRecv;    
    int needUpdateProc = 0;

    memset(&stDataRecv,0,sizeof(recvDataEleCtl));
    
    //printf("in ack_frame_proc, rcw is %d\n",rcw);
    switch(rcw)
    {
        case MCF:
        if(frameAck->len == 2)
        {
            tmpShort = frameAck->data[0] + (frameAck->data[1]<<8);
            
            if((tmpShort&UIM_MCFG_OPEN) == UIM_MCFG_OPEN)
            {              

                //do nothing;
                printf("MCF set success,sid[%d],%02x,%02x,%02x\n",frameAck->sid,
                frameAck->data[0],frameAck->data[1],frameAck->data[0]&0x10);
            }
            else
            {
                unsigned char dataOut[2];
                
                uim_data dataRet;
                struct can_frame frameToSend;
                uim_frame dataRead;
                printf("tmpShort&UIM_MCFG_OPEN:%04x\n",tmpShort&UIM_MCFG_OPEN);
                tmpShort = tmpShort|UIM_MCFG_OPEN;
                dataOut[0] = tmpShort&0x00ff;
                dataOut[1] = (tmpShort>>8)&0x00ff;
                
                printf("set MCF, sid[%d],%02x,%02x\n",frameAck->sid,dataOut[0],dataOut[1]);
                format_cmd(1,MCF,frameAck->sid,2, dataOut,&dataRet);
                encap_uim_data(&dataRet,&frameToSend);     
                this->WriteCanPort(&frameToSend);
                /* code */
            }
            
        }
        break;
        case NOTIFY_MOVE_END:
        {
            int tempCurPos = 0;
            char *pdata = (char *)&tempCurPos;//(unsigned char*)(&(uimCtlArr[frameAck->sid - 10].currPos));
            
            int tempLen = frameAck->len>4?4:frameAck->len;

            memcpy(&tempCurPos,frameAck->data,frameAck->len);
            if(getELeCtlCurrPos(frameAck->sid) != tempCurPos)
            {
                setELeCtlCurrPos(frameAck->sid,tempCurPos);
            }

            if(getELeCtlStatus(frameAck->sid) == MOVING 
             || getELeCtlStatus(frameAck->sid) == RECV_POS 
             || getELeCtlStatus(frameAck->sid) == INIT_AWAY_ORIGING
             || getELeCtlStatus(frameAck->sid) == MOVE_RELATIVE )
            {
                

                printf("sid[%d], move end, curPos[%d],status[%d]\n",frameAck->sid,tempCurPos,getELeCtlStatus(frameAck->sid));
                
                
                if(INIT_AWAY_ORIGING!=getELeCtlStatus(frameAck->sid))
                {
                    sendAckToGrpc(0,0,frameAck->sid,-1);
                    needUpdateProc = 1;
                    stDataRecv.frameType = CHECK_MOVE_END;
                }
                setELeCtlStatus(frameAck->sid,FINISHED);
                
            }
            else 
            {
                printf("sid[%d],status[%d],MOVEING[%d]\n",frameAck->sid,getELeCtlStatus(frameAck->sid),MOVING);
            }
        
            // pthread_mutex_lock(&mutexWriteDp8Z);
            // if(g_fp_z && frameAck->sid == 15)
            // {
            //     fclose(g_fp_z);
            //     g_fp_z = NULL;
            // }
            // pthread_mutex_unlock(&mutexWriteDp8Z);

            
        }        
        break;
        case S1_EDGE:
        {
            //printf("S1_EDGE,ctlCode:%d,status:%d\n",ctlCode,getELeCtlStatus(frameAck->sid));
            int eleCtlStatus = getELeCtlStatus(frameAck->sid);
            if(/*ctlCode==1 && */eleCtlStatus == RET_BACKING || INIT_TO_ORIG_1 == eleCtlStatus)
            {
                printf("ret back finished,sid[%d]\n",frameAck->sid);
                
                uim_data dataRet;
                struct can_frame frameToSend;
                uim_frame dataRead;
                unsigned char dataOut[4] = {0x00,0x00,0x00,0x00};
                //uimCtlArr[frameAck->sid - 10].status = FINISHED;
                if(RET_BACKING == eleCtlStatus)
                {
                    setELeCtlStatus(frameAck->sid,FINISHED);
                    needUpdateProc = 1;
                    stDataRecv.frameType = CHECK_RET_BACK_END;
                }
                else
                {
                    setELeCtlStatus(frameAck->sid,INIT_TO_ON_ORIG_1);
                }
                
                
                format_cmd(1,ORG,frameAck->sid,4, dataOut,&dataRet);
                encap_uim_data(&dataRet,&frameToSend);     
                this->WriteCanPort(&frameToSend);
                //uimCtlArr[frameAck->sid - 10].currPos = 0;
                setELeCtlCurrPos(frameAck->sid,0);

                //uimCtlArr[frameAck->sid - 10].status = FINISHED;
                
                //send the ack msg to the caller.
                // msgQueueAck stMsgQueueAck;
                // memset(&stMsgQueueAck,0,sizeof(msgQueueAck));
                // stMsgQueueAck.type = 3;
                // stMsgQueueAck.msg.statusCode = 0;
                //if(msgsnd(this->getKeyId_FrameMQ(), (void*)&stMsgQueueAck, sizeof(msgAck), 0) < 0)
                //{
                //     printf("send ack failed\n");
                //}
            }
            else if(getELeCtlStatus(frameAck->sid) == MOVING)
            {
                
                printf("moving accross sensor, sid[%d]\n",frameAck->sid);
                
                if(getELeCtlRetflag(frameAck->sid)==ctlCode)
                {
                    uim_data dataRet;
                    struct can_frame frameToSend;
                    uim_frame dataRead;
                    unsigned char dataOut[4] = {0x00,0x00,0x00,0x00};

                    setELeCtlStatus(frameAck->sid,FINISHED);
                    format_cmd(1,ORG,frameAck->sid,4, dataOut,&dataRet);
                    encap_uim_data(&dataRet,&frameToSend);     
                    this->WriteCanPort(&frameToSend);
                    setELeCtlCurrPos(frameAck->sid,0);


                    needUpdateProc = 1;
                    stDataRecv.frameType = CHECK_MOVE_END;
                }

            }

        }

        break;
        case SFB:
        if(getELeCtlStatus(frameAck->sid) == CHECKING_RET || getELeCtlStatus(frameAck->sid) == CHECKING_RET_INIT)
        {
            printf("ret back checking finished,sid[%d],status[%d]\n",frameAck->sid,
            getELeCtlStatus(frameAck->sid));
            
            //if up side of signal indicating origination,
            //else down side of signal indicating origination.
            if(frameAck->len ==2 && (frameAck->data[1]&0x10) == 0x00 )
            {
                //uimCtlArr[frameAck->sid - 10].status = FINISHED;
                if(getELeCtlRetflag(frameAck->sid) == STOP_WHEN_UP)
                {
                   setELeCtlStatus(frameAck->sid,FINISHED);
                }
                else if(getELeCtlRetflag(frameAck->sid) == STOP_WHEN_DOWN)
                {
                   if(getELeCtlStatus(frameAck->sid) == CHECKING_RET)
                   {
                       needUpdateProc = 1;
                       stDataRecv.frameType = CHECK_RET_BACK_END;
                   }                  
                   uim_data dataRet;
                   struct can_frame frameToSend;
                   unsigned char dataOut[4] = {0x00,0x00,0x00,0x00};
                   

                   //tmpPosCmd = SPD;
                   format_cmd(1,SPD,frameAck->sid,4, dataOut,&dataRet);//set speed to 0
                   encap_uim_data(&dataRet,&frameToSend);     
                   this->WriteCanPort(&frameToSend);

                   format_cmd(1,ORG,frameAck->sid,4, dataOut,&dataRet);
                   encap_uim_data(&dataRet,&frameToSend);     
                   this->WriteCanPort(&frameToSend);

                   setELeCtlStatus(frameAck->sid,ALREADY_ON_ORIGIN); 
                }
                
            }
            else if(frameAck->len ==2 && (frameAck->data[1]&0x10) == 0x10)
            {
                if(getELeCtlRetflag(frameAck->sid) == STOP_WHEN_UP)
                {
                    
                    if(getELeCtlStatus(frameAck->sid) == CHECKING_RET)
                    {
                        needUpdateProc = 1;
                        stDataRecv.frameType = CHECK_RET_BACK_END;
                    }


                    uim_data dataRet;
                    struct can_frame frameToSend;
                    unsigned char dataOut[4] = {0x00,0x00,0x00,0x00};

                    format_cmd(1,SPD,frameAck->sid,4, dataOut,&dataRet);//set speed to 0
                    encap_uim_data(&dataRet,&frameToSend);     
                    this->WriteCanPort(&frameToSend);

                    format_cmd(1,ORG,frameAck->sid,4, dataOut,&dataRet);
                    encap_uim_data(&dataRet,&frameToSend);     
                    this->WriteCanPort(&frameToSend);

                    setELeCtlStatus(frameAck->sid,ALREADY_ON_ORIGIN); 
                }
                else if(getELeCtlRetflag(frameAck->sid) == STOP_WHEN_DOWN)
                {
                   setELeCtlStatus(frameAck->sid,FINISHED);
                }               
                
            }
            
        }
        else if(getELeCtlStatus(frameAck->sid) == RET_BACKING
              ||getELeCtlStatus(frameAck->sid) == INIT_TO_ORIG_1)
        {
            if(frameAck->len ==2)
            {
                int sensorHigh = ((frameAck->data[1]&0x10)!=0)?1:0;
                
                //getELeCtlRetflag(frameAck->sid) == STOP_WHEN_UP && sensorHigh==1
                //getELeCtlRetflag(frameAck->sid) == STOP_WHEN_DOWN && sensorHigh==0
                if(getELeCtlRetflag(frameAck->sid) == sensorHigh)
                {
                    uim_data dataRet;
                    struct can_frame frameToSend;
                    uim_frame dataRead;
                    unsigned char dataOut[4] = {0x00,0x00,0x00,0x00};
                    //uimCtlArr[frameAck->sid - 10].status = FINISHED;

                    format_cmd(1,SPD,frameAck->sid,4, dataOut,&dataRet);//set speed to 0
                    encap_uim_data(&dataRet,&frameToSend);     
                    this->WriteCanPort(&frameToSend);
                    
                    format_cmd(1,ORG,frameAck->sid,4, dataOut,&dataRet);
                    encap_uim_data(&dataRet,&frameToSend);     
                    this->WriteCanPort(&frameToSend);

                    setELeCtlStatus(frameAck->sid,FINISHED);

                    // msgQueueAck stMsgQueueAck;
                    // memset(&stMsgQueueAck,0,sizeof(msgQueueAck));
                    // stMsgQueueAck.type = QUEUE_MSG_TYPE_GRPC;
                    // stMsgQueueAck.msg.statusCode = 0;
                    if(getELeCtlStatus(frameAck->sid) != INIT_TO_ORIG_1)
                    {
                       needUpdateProc = 1;
                       stDataRecv.frameType = CHECK_RET_BACK_END;
                    }


                    printf("ret back finished, query\n");

                    //uimCtlArr[frameAck->sid - 10].currPos = 0;
                    setELeCtlCurrPos(frameAck->sid,0);
                }
                
                
                //if(msgsnd(this->getKeyId_FrameMQ(), (void*)&stMsgQueueAck, sizeof(msgAck), 0) < 0)
                //{
                //    printf("send ack failed\n");
                //}
            }
            else if(frameAck->len ==2 && (frameAck->data[1]&0x10) == 0x00)
            {
                if(getELeCtlRetflag(frameAck->sid) == STOP_WHEN_DOWN)
                {
                    uim_data dataRet;
                    struct can_frame frameToSend;
                    uim_frame dataRead;
                    unsigned char dataOut[4] = {0x00,0x00,0x00,0x00};
                    //uimCtlArr[frameAck->sid - 10].status = FINISHED;

                    format_cmd(1,SPD,frameAck->sid,4, dataOut,&dataRet);//set speed to 0
                    encap_uim_data(&dataRet,&frameToSend);     
                    this->WriteCanPort(&frameToSend);

                    format_cmd(1,ORG,frameAck->sid,4, dataOut,&dataRet);
                    encap_uim_data(&dataRet,&frameToSend);     
                    this->WriteCanPort(&frameToSend);
                    setELeCtlStatus(frameAck->sid,FINISHED);
                    // msgQueueAck stMsgQueueAck;
                    // memset(&stMsgQueueAck,0,sizeof(msgQueueAck));
                    // stMsgQueueAck.type = QUEUE_MSG_TYPE_GRPC;
                    // stMsgQueueAck.msg.statusCode = 0;

                    needUpdateProc = 1;
                    stDataRecv.frameType = CHECK_RET_BACK_END;

                    printf("ret back finished, query\n");

                    //uimCtlArr[frameAck->sid - 10].currPos = 0;
                    setELeCtlCurrPos(frameAck->sid,0);
                }
            }
            
        }
        else if(getELeCtlStatus(frameAck->sid) == GETTING_SWITH_STATUS)
        {
           int * sensorHigh = (int *)getELeCtlBackDataPtr(frameAck->sid);
           if(sensorHigh)
           {
              * sensorHigh = ((frameAck->data[1]&0x10)!=0)?1:0;
           }           
           setELeCtlStatus(frameAck->sid,FINISHED);
        }
        else
        {
            printf("SFB recv, sid[%d],status[%d]\n",frameAck->sid,getELeCtlStatus(frameAck->sid));
        }
        break;

        case POS:
        case QEC:
        {
            setELeCtlStatus(frameAck->sid,RECV_POS);
            
            int tempCurPos = 0;
            memcpy(&tempCurPos,frameAck->data,frameAck->len);
            setELeCtlCurrPos(frameAck->sid,tempCurPos);
            
            printf("ack of query pos, sid[%d], currPos[%d]\n",frameAck->sid,tempCurPos);
            
            pthread_mutex_lock(&mutexWriteDp8Z);
            if(g_fp_z && frameAck->sid == 15)
            {
                char buffData[64] = {0};
                sprintf(buffData,"%d\r\n",tempCurPos);
                fwrite(buffData,strlen(buffData),1,g_fp_z);
            }
            pthread_mutex_unlock(&mutexWriteDp8Z);
        }
        break;
        case ACK_COMMON:
        {
            unsigned char current = 0;
            if(frameAck->len >= 2)
            {
                current = ((frameAck->data[1]&0x07) << 4) 
                                     + ((frameAck->data[0] & 0xf0)>>4);
            }                                    
            printf("ACK_COMMON received, len:%d, current:%d\n",frameAck->len,current);
        }
        break;
        default:
        break;
    }

    if(needUpdateProc==1)
    {
        //need param when update status.          
    
        stDataRecv.sid = frameAck->sid;


        if(getELeCtlAckFlag(frameAck->sid) == MSG_FROM_LUA)
        {
            sendAckToLua(&stDataRecv);
        }

        //   else 
        //   {
        //       printf("sid:%d,ackflag:%d\n",frameAck->sid,getELeCtlAckFlag(frameAck->sid));
        //   }

        if(g_ptrObManage)
        {
            //stDataRecv.frameType = rcw;
            g_ptrObManage->update(&stDataRecv);
        }
        
    }
}

void CUim_MainCtl::uim_ret_back(unsigned int sid,int direct,int loopType)
{
  //1,set mcfg, enable S1IE, MCF
  uim_data dataRet;
  struct can_frame frameToSend;
  uim_frame dataRead;

    format_cmd(1,ENA,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

  //format_cmd(1,MCF,sid,0, NULL,&dataRet);
  //encap_uim_data(&dataRet,&frameToSend);     
  //this->WriteCanPort(&frameToSend);
  //2,set S1FACT 0x04; SCF
  unsigned char s1con[4] = {0x40,0x00,0x00};
  format_cmd(1,SCF,sid,3, s1con,&dataRet);
  encap_uim_data(&dataRet,&frameToSend);     
  this->WriteCanPort(&frameToSend);

  //3, set sample module
  unsigned char interval[4] = {0x00,0x00,0x00};
  format_cmd(1,STG,sid,3, interval,&dataRet);
  encap_uim_data(&dataRet,&frameToSend);     
  this->WriteCanPort(&frameToSend);

  //4,STOn binding act to S1IE

  //5,set the speed
  int speed = 1000;
  unsigned char speedCode[4] = {speed&0x00ff,
            (speed>>8)&0x00ff,(speed>>16)&0x00ff,(speed>>24)&0x00ff};
  format_cmd(1,SPD,sid,4, speedCode,&dataRet);
  encap_uim_data(&dataRet,&frameToSend);     
  this->WriteCanPort(&frameToSend);

    int tmpPos = getELeCtlMaxD(sid);
    
    if(getELeCtlRetdir(sid)==0)
    {
        tmpPos = 0 - tmpPos;
    }
    //tmpPos = -20000;
    unsigned char dataPos[4]; 
    dataPos[0] = tmpPos&0x00ff;
    dataPos[1] = (tmpPos>>8)&0x00ff;
    dataPos[2] = (tmpPos>>16)&0x00ff;
    dataPos[3] = (tmpPos>>24)&0x00ff;

    format_cmd(1,STP,sid,4, dataPos,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);


    //uimCtlArr[sid-10].status = RET_BACKING;
    setELeCtlStatus(sid,RET_BACKING);

  //sleep(20);

  //format_cmd(1,OFF,sid,0, NULL,&dataRet);
  //encap_uim_data(&dataRet,&frameToSend);     
  //this->WriteCanPort(&frameToSend);

}

unsigned char getELeCtlStatus(unsigned char sid)
{
#ifdef _QPCR
    if(sid>=40 && sid<(40+MAX_UIM_CTL_NUM_PCR))
    {
        //printf("get qpcr ele status sid:%d\n",sid);
        return uimCtlArr_pcr[sid - 40].status;
    }
#else
    if(sid>=10 && sid<(10+MAX_UIM_CTL_NUM))
    {
        return uimCtlArr[sid - 10].status;
    }
#endif
    return NOT_START;
}

void setELeCtlStatus(unsigned char sid, unsigned char statusIn)
{
#ifdef _QPCR
    if(sid>=40 && sid<(40+MAX_UIM_CTL_NUM_PCR))
    {
        //printf("set sid[%d] status to [%d]\n",sid,statusIn);
        uimCtlArr_pcr[sid - 40].status = statusIn;
    }
#else
    if(sid>=10 && sid<(10+MAX_UIM_CTL_NUM))
    {
        uimCtlArr[sid - 10].status = statusIn;
    }
#endif
}

void setELeCtlBackData(unsigned char sid, void * dataCallBackIn)
{
#ifdef _QPCR
    if(sid>=40 && sid<(40+MAX_UIM_CTL_NUM_PCR))
    {
        //printf("set sid[%d] status to [%d]\n",sid,statusIn);
        uimCtlArr_pcr[sid - 40].dataCallBack= dataCallBackIn;
    }
#else
    if(sid>=10 && sid<(10+MAX_UIM_CTL_NUM))
    {
        uimCtlArr[sid - 10].dataCallBack = dataCallBackIn;
    }
#endif
}

void * getELeCtlBackDataPtr(unsigned char sid)
{
#ifdef _QPCR
    if(sid>=40 && sid<(40+MAX_UIM_CTL_NUM_PCR))
    {
        //printf("set sid[%d] status to [%d]\n",sid,statusIn);
        return uimCtlArr_pcr[sid - 40].dataCallBack;
    }
#else
    if(sid>=10 && sid<(10+MAX_UIM_CTL_NUM))
    {
        return uimCtlArr[sid - 10].dataCallBack;
    }
#endif    
}

unsigned char getELeCtlRetdir(unsigned char sid)
{
#ifdef _QPCR
    if(sid>=40 && sid<(40+MAX_UIM_CTL_NUM_PCR))
    {
        return uimCtlArr_pcr[sid - 40].ret_dir;
    }
#else
    if(sid>=10 && sid<(10+MAX_UIM_CTL_NUM))
    {
        return uimCtlArr[sid - 10].ret_dir;
    }
#endif
    return NOT_START;
}


unsigned char getELeCtlRetflag(unsigned char sid)
{
#ifdef _QPCR
    if(sid>=40 && sid<(40+MAX_UIM_CTL_NUM_PCR))
    {
        return uimCtlArr_pcr[sid - 40].retFlag;
    }
#else
    if(sid>=10 && sid<(10+MAX_UIM_CTL_NUM))
    {
        return uimCtlArr[sid - 10].retFlag;
    }
#endif
    return STOP_WHEN_UP; 
}

unsigned int getELeCtlMaxD(unsigned char sid)
{
#ifdef _QPCR
    if(sid>=40 && sid<(40+MAX_UIM_CTL_NUM_PCR))
    {
        return uimCtlArr_pcr[sid - 40].max_distance;
    }
#else
    if(sid>=10 && sid<(10+MAX_UIM_CTL_NUM))
    {
        return uimCtlArr[sid - 10].max_distance;
    }
#endif
    return 0;
}

int getELeCtlCurrPos(unsigned char sid)
{
#ifdef _QPCR
    if(sid>=40 && sid<(40+MAX_UIM_CTL_NUM_PCR))
    {
        return uimCtlArr_pcr[sid - 40].currPos;
    }
#else
    if(sid>=10 && sid<(10+MAX_UIM_CTL_NUM))
    {
        return uimCtlArr[sid - 10].currPos;
    }
#endif
    return NOT_START;
}

void setELeCtlCurrPos(unsigned char sid,int curPos)
{
#ifdef _QPCR
    if(sid>=40 && sid<(40+MAX_UIM_CTL_NUM_PCR))
    {
        uimCtlArr_pcr[sid - 40].currPos = curPos;
    }
#else
    if(sid>=10 && sid<(10+MAX_UIM_CTL_NUM))
    {
        uimCtlArr[sid - 10].currPos = curPos;
    }
#endif
}

unsigned char getELeCtlLoopType(unsigned char sid)
{
#ifdef _QPCR
    if(sid>=40 && sid<(40+MAX_UIM_CTL_NUM_PCR))
    {
        return uimCtlArr_pcr[sid - 40].loopType;
    }
#else
    if(sid>=10 && sid<(10+MAX_UIM_CTL_NUM))
    {
        return uimCtlArr[sid - 10].loopType;
    }
#endif
    return NOT_START;
}

unsigned char getELeCtlAckFlag(unsigned char sid)
{
#ifdef _QPCR
    if(sid>=40 && sid<(40+MAX_UIM_CTL_NUM_PCR))
    {
        return uimCtlArr_pcr[sid - 40].ackFlag;
    }
#else
    if(sid>=10 && sid<(10+MAX_UIM_CTL_NUM))
    {
        return uimCtlArr[sid - 10].ackFlag;
    }
#endif
    return MSG_FROM_TEST;
}
void setELeCtlAckFlag(unsigned char sid, unsigned char ackFlagIn)
{
#ifdef _QPCR
    if(sid>=40 && sid<(40+MAX_UIM_CTL_NUM_PCR))
    {
        uimCtlArr_pcr[sid - 40].ackFlag = ackFlagIn;
    }
#else
    if(sid>=10 && sid<(10+MAX_UIM_CTL_NUM))
    {
        uimCtlArr[sid - 10].ackFlag = ackFlagIn;
    }
#endif   
}
void CUim_MainCtl::setSensorAction(int sid, int isEnable)
{
    unsigned char s1con[4] = {0xc0,0x00,0x00};// stop and set the pos to 0
    uim_data dataRet;
    struct can_frame frameToSend;
    if(isEnable)
    {
        if(getELeCtlRetflag(sid) == STOP_WHEN_UP)
        {
            // if( sid==17 )//|| sid==52 || sid==53)sid==45 || sid==46 ||
            // {
            //     s1con[0] = 0xb0; //decrease the speed to zero 
            // }
            // if(sid==51)
            // {
            //     s1con[1] = 0xc0; 
            // }
        
        }
        else if(getELeCtlRetflag(sid) == STOP_WHEN_DOWN)
        {
             s1con[0] = 0x0c; 
            // if( sid==17 )//|| sid==52 || sid==53)sid==45 || sid==46 ||
            // {
            //     s1con[0] = 0x0B; //decrease the speed to zero 
            // }
            // if(sid==51)
            // {
            //     s1con[1] = 0x0C; 
            // }
        }

    }
    else
    {
        //unsigned char s1con_dis[3] = {0x11,0x00,0x00};
        s1con[0] = 0x11;
        s1con[1] = 0x00;
        s1con[2] = 0x00;
    }
    
    
    
    format_cmd(1,SCF,sid,3, s1con,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
}

//when motor moving away origin slowly, in the retback procedure.
void CUim_MainCtl::setSensorRevert(int sid,int steps)
{
    unsigned char s1con[4] = {0xd0,0x00,0x00};// move ahead some steps
    uim_data dataRet;
    struct can_frame frameToSend;

    unsigned char stoData = 0;

    //1, disable the controller
    format_cmd(1,OFF,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);


    //2, set the inc pos, STP
    unsigned char pos_step[4] = {steps&0x00ff, (steps>>8)&0x00ff, (steps>>16)&0x00ff, (steps>>24)&0x00ff};
    format_cmd(1,STP,sid,4, pos_step,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);


    //3, set the sensor action reverted, stop edge revert.

    if(getELeCtlRetflag(sid) == STOP_WHEN_UP)
    {
        // if( sid==17 )//|| sid==52 || sid==53)sid==45 || sid==46 ||
        // {
        //     s1con[0] = 0xb0; //decrease the speed to zero 
        // }
        // if(sid==51)
        // {
        //     s1con[1] = 0xc0; 
        // }
        s1con[0] = 0x0d;//move ahead some steps

        stoData = 3;//bind to down edge
    
    }
    else if(getELeCtlRetflag(sid) == STOP_WHEN_DOWN)
    {
        //s1con[0] = 0x0c; 
        // if( sid==17 )//|| sid==52 || sid==53)sid==45 || sid==46 ||
        // {
        //     s1con[0] = 0x0B; //decrease the speed to zero 
        // }
        // if(sid==51)
        // {
        //     s1con[1] = 0x0C; 
        // }
        stoData = 2;//bind action to S1 up edge
    }

    format_cmd(1,STO,sid,1, &stoData,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
     
    
    format_cmd(1,SCF,sid,3, s1con,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
}

void CUim_MainCtl::uim_ret_back(void * moveParamIn)
{
  //1,set mcfg, enable S1IE, MCF
  uim_data dataRet;
  struct can_frame frameToSend;
  uim_frame dataRead;

  int count = 0;
  move_param * stMoveParam = (move_param *)moveParamIn;
  unsigned char sid = stMoveParam->sid;

  #define MAX_WAIT_CHECKING_COUNT 80

//   unsigned char cmdCode = POS;
//   if(uimCtlArr[sid-10].loopType == CLOSE_LOOP)
//   {
//       cmdCode = QEC;
//   }

    format_cmd(1,ENA,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    setELeCtlStatus(sid,CHECKING_RET);
  //1, get current switch status
    format_cmd(1,SFB,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
    //uimCtlArr[sid-10].status = CHECKING_RET;

    

    int comValue = MAX_WAIT_CHECKING_COUNT/3;
    count = 0;
    while(getELeCtlStatus(sid) == CHECKING_RET)
    {
        count++;
        usleep(50000);
        if(count>MAX_WAIT_CHECKING_COUNT)
        {
            break;
        }

        if(count%comValue==0)
        {
            printf("send SFB\n");
            this->WriteCanPort(&frameToSend);
        }

        //printf("in loop,count:%d\n",count);
    }

    if(count>MAX_WAIT_CHECKING_COUNT)
    {
        printf("sid[%d],check ret back timeout,status[%d]\n",sid,getELeCtlStatus(sid));
        if(getELeCtlAckFlag(sid) == MSG_FROM_GRPC)
        {
           sendAckToGrpc(1,ACT_TIMEOUT_APP,sid,-1);
        }
        else if(getELeCtlAckFlag(sid) == MSG_FROM_LUA)
        {
           recvDataEleCtl stDataRecv;

           memset(&stDataRecv,0,sizeof(recvDataEleCtl));
           stDataRecv.sid = sid;
           stDataRecv.frameType = CHECK_ACT_TIMEOUT;
           sendAckToLua(&stDataRecv);
        }       

        return;
    }
    else if(getELeCtlStatus(sid) == ALREADY_ON_ORIGIN)
    {
        printf("sid[%d],already on origin point\n",sid);
        //sendAckToGrpc(0,0,sid,-1);
        setELeCtlCurrPos(sid,0);
        if(getELeCtlAckFlag(sid) == MSG_FROM_LUA)
        {
           recvDataEleCtl stDataRecv;

           memset(&stDataRecv,0,sizeof(recvDataEleCtl));
           stDataRecv.sid = sid;
           stDataRecv.frameType = CHECK_RET_BACK_END;
           sendAckToLua(&stDataRecv);
        }
        else //if(getELeCtlAckFlag(sid) == MSG_FROM_GRPC)
        {
           sendAckToGrpc(0,0,sid,-1);
        } 
        
        
        return;
    }

     //printf("act ret back now!,sid[%d],111\n",sid);

  //format_cmd(1,MCF,sid,0, NULL,&dataRet);
  //encap_uim_data(&dataRet,&frameToSend);     
  //this->WriteCanPort(&frameToSend);



  //2,set S1FACT 0x04; SCF
//   unsigned char s1con[4] = {0xc0,0x00,0x00};// stop and set the pos to 0

//   if(getELeCtlRetflag(sid) == STOP_WHEN_UP)
//   {
//       if( sid==17 )//|| sid==52 || sid==53)sid==45 || sid==46 ||
//       {
//           s1con[0] = 0xb0; //decrease the speed to zero 
//       }
//       if(sid==51)
//       {
//           s1con[1] = 0xc0; 
//       }
      
//   }
//   else if(getELeCtlRetflag(sid) == STOP_WHEN_DOWN)
//   {
//       s1con[0] = 0x0c; 
//       if( sid==17 )//|| sid==52 || sid==53)sid==45 || sid==46 ||
//       {
//             s1con[0] = 0x0B; //decrease the speed to zero 
//       }
//       if(sid==51)
//       {
//             s1con[1] = 0x0C; 
//       }
//   }
  
//   format_cmd(1,SCF,sid,3, s1con,&dataRet);
//   encap_uim_data(&dataRet,&frameToSend);     
//   this->WriteCanPort(&frameToSend);
     setSensorAction(sid,1);

  //3, set sample module
  unsigned char interval[4] = {0x00,0x00,0x00};
  format_cmd(1,STG,sid,3, interval,&dataRet);
  encap_uim_data(&dataRet,&frameToSend);     
  this->WriteCanPort(&frameToSend);

  //4,STOn binding act to S1IE
//printf("act ret back now!,sid[%d],222\n",sid);
   //5.1 set the incease speed
   int inc_speed = stMoveParam->inc_speed;
   printf("ret back, set inc_speed to %d\n",inc_speed);
   unsigned char incSpeedChar[4] = {inc_speed&0x00ff,
            (inc_speed>>8)&0x00ff,(inc_speed>>16)&0x00ff,(inc_speed>>24)&0x00ff 
   };
  
  format_cmd(1,MAC,sid,4, incSpeedChar,&dataRet);
  encap_uim_data(&dataRet,&frameToSend);     
  this->WriteCanPort(&frameToSend);

  //5.2 set the decrease speed
  int dec_speed = stMoveParam->dec_speed;
  printf("ret back, set dec_speed to %d\n",dec_speed);
   unsigned char decSpeedChar[4] = {dec_speed&0x00ff,
            (dec_speed>>8)&0x00ff,(dec_speed>>16)&0x00ff,(dec_speed>>24)&0x00ff 
   };
  
  format_cmd(1,MDE,sid,4, decSpeedChar,&dataRet);
  encap_uim_data(&dataRet,&frameToSend);     
  this->WriteCanPort(&frameToSend);
  //printf("act ret back now!,sid[%d],333\n",sid);

    int tmpPos = 0;//(int)uimCtlArr[sid-10].max_distance;
    
     if(getELeCtlRetdir(sid)==0)
     {
         tmpPos = 0 - tmpPos;
     }
    //tmpPos = -20000;
    //printf("ret back, set pos to %d\n",tmpPos);
    unsigned char dataPos[4] = {0x00,0x00,0x00,0x00}; 
     dataPos[0] = tmpPos&0x00ff;
     dataPos[1] = (tmpPos>>8)&0x00ff;
     dataPos[2] = (tmpPos>>16)&0x00ff;
     dataPos[3] = (tmpPos>>24)&0x00ff;

    format_cmd(1,STP,sid,4, dataPos,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

  //5,set the speed
  int speed = stMoveParam->speed;
  if(getELeCtlRetdir(sid) == 0)
  {
      speed = 0 - speed;
  }
  printf("ret back, set speed to %d\n",speed);
  unsigned char speedCode[4] = {speed&0x00ff,
            (speed>>8)&0x00ff,(speed>>16)&0x00ff,(speed>>24)&0x00ff};
  format_cmd(1,SPD,sid,4, speedCode,&dataRet);
  encap_uim_data(&dataRet,&frameToSend);     
  this->WriteCanPort(&frameToSend);

    

    printf("act ret back now!,sid[%d]\n",sid);
    //uimCtlArr[sid-10].status = RET_BACKING;
    setELeCtlStatus(sid,RET_BACKING);
    int currPos = getELeCtlCurrPos(sid);
    currPos = (currPos>0)?currPos:(0-currPos);

    
    int maxCnt = MAX_WAIT_CHECKING_COUNT;
    if(stMoveParam->speed && currPos)
    {
        maxCnt = (currPos*10)/(stMoveParam->speed);
    }
    count = 0;
    while(getELeCtlStatus(sid) == RET_BACKING)
    {
        count++;
        usleep(100000);
        if(count>maxCnt)
        {
            break;
        }
        //printf("in loop,count:%d\n",count);
    }

    count = 0;
    while(getELeCtlStatus(sid) == RET_BACKING)
    {
        count++;
        usleep(200000);
        if(count>MAX_WAIT_CHECKING_COUNT)
        {
            break;
        }
        
        format_cmd(1,SFB,sid,0, NULL,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);
        //printf("in loop,count:%d\n",count);
    }

    if(count>MAX_WAIT_CHECKING_COUNT)
    {
        printf("wait ret back timeout\n");       
        return;
    }
    else
    {
        sendAckToGrpc(0,0,sid,-1);
    }

  //sleep(20);

  //format_cmd(1,OFF,sid,0, NULL,&dataRet);
  //encap_uim_data(&dataRet,&frameToSend);     
  //this->WriteCanPort(&frameToSend);

}

void * uimActThreadFunc(void* lparam)
{
    
    actThreadParam * paramIn = (actThreadParam *)lparam;
    int flag = 1;

    if(NULL==paramIn)
    {
        pthread_detach(pthread_self());
        return NULL;
    }
    
    CUim_MainCtl * pUimCanInterface = paramIn->pCanInterface;
    move_param * ptrActParam = (move_param *)paramIn->actParam;
    printf("paramAll:%p,actParam:%p\n",lparam,paramIn->actParam);
    printf("enter in uimActThreadFunc, opcode is [%d],sid[%d]\n",paramIn->opcode,ptrActParam->sid);
    if(paramIn->opcode == RET_BACK)
    {
            move_param stMoveParam;
            memcpy(&stMoveParam,ptrActParam,sizeof(move_param));// = (move_param *)paramIn->actParam;
            printf("actParam to deleted\n");
            delete ptrActParam;
            printf("actParam deleted\n");
            paramIn->actParam = NULL;
            pUimCanInterface->uim_ret_back(&stMoveParam);
    }
    else if(paramIn->opcode == MOVE)
    {
            move_param stMoveParam;
            memcpy(&stMoveParam,ptrActParam,sizeof(move_param));
            delete ptrActParam;
            paramIn->actParam = NULL;
            pUimCanInterface->uim_move(&stMoveParam);
    }
    else if(paramIn->opcode == MOVE_RELATIVE)
    {
            move_param stMoveParam;
            memcpy(&stMoveParam,ptrActParam,sizeof(move_param));
            delete ptrActParam;
            paramIn->actParam = NULL;
            pUimCanInterface->uim_move_relative(&stMoveParam);        
    }

    else if(paramIn->opcode == UIM_MOTOR_INIT)
    {
        move_param stMoveParam;
        memcpy(&stMoveParam,ptrActParam,sizeof(move_param));
        delete ptrActParam;
        paramIn->actParam = NULL;
        pUimCanInterface->uim_motor_init1(stMoveParam.sid);        
    }
    else if(UIM_GOZERO == paramIn->opcode)
    {
        move_param stMoveParam;
        memcpy(&stMoveParam,ptrActParam,sizeof(move_param));
        delete ptrActParam;
        paramIn->actParam = NULL;
        pUimCanInterface->uim_motor_retback(&stMoveParam,stMoveParam.sid);           
    }
    
    delete paramIn;

    pthread_detach(pthread_self());
    pthread_exit( NULL );
    
}

int CUim_MainCtl::checkMoveParam(void * moveParamIn)
{
    move_param * stMoveParam = (move_param *)moveParamIn;
    unsigned char sid = stMoveParam->sid;
    unsigned int speed = stMoveParam->speed;
    unsigned int inc_speed = stMoveParam->inc_speed;
    unsigned int dec_speed = stMoveParam->dec_speed;
    unsigned int position = stMoveParam->pos;
    unsigned int move_dir = stMoveParam->dir;

    int endPos = 0; 
    int maxDistance = getELeCtlRetdir(sid)?(0-getELeCtlMaxD(sid)):getELeCtlMaxD(sid);
    
    int retVal = 0;
    //if position is larger than speed, no ack of move end received
    if(position<speed)
    {
        stMoveParam->speed = position;
        speed = position;
    }


    if(move_dir==getELeCtlRetdir(sid) )
    {
        if(getELeCtlRetdir(sid))
        {
           endPos  = getELeCtlCurrPos(sid) + position; 
        }
        else
        {
            /* code */
            endPos  = getELeCtlCurrPos(sid) - position; 
        }        
    }
    else
    {
        /* code */
        if(getELeCtlRetdir(sid))
        {
           endPos  = getELeCtlCurrPos(sid) - position; 
        }
        else
        {
            /* code */
            endPos  = getELeCtlCurrPos(sid) + position; 
        }
    }
    
    printf("curPos[%d],posToMove[%d],dir[%d],endPos[%d],",
           getELeCtlCurrPos(sid),position,move_dir,endPos);
    if(maxDistance<0)
    {
        if(endPos<=0 && endPos >=maxDistance)
        {
            retVal = 1;
        }
        else 
        {
            retVal = 0;
        }
    }
    else
    {
        if(endPos>=0 && endPos <=maxDistance)
        {
            retVal = 1;
        }
        else 
        {
            retVal = 0;
        }
        /* code */
    }  
    printf("valid[%d]\n",retVal);
    return retVal;
}

int CUim_MainCtl::checkMoveParam_absolutePos(void * moveParamIn)
{
    move_param * stMoveParam = (move_param *)moveParamIn;
    unsigned char sid = stMoveParam->sid;
    unsigned int speed = stMoveParam->speed;
    unsigned int inc_speed = stMoveParam->inc_speed;
    unsigned int dec_speed = stMoveParam->dec_speed;
    unsigned int position = stMoveParam->pos;
    unsigned int move_dir = stMoveParam->dir;

    int endPos = 0; 
    int maxDistance = getELeCtlRetdir(sid)?(0-getELeCtlMaxD(sid)):getELeCtlMaxD(sid);
    
    int retVal = 0;
    //if position is larger than speed, no ack of move end received
    // if(position<speed)
    // {
    //     stMoveParam->speed = position;
    //     speed = position;
    // }

    if(getELeCtlRetdir(sid)==NO_RET_BACK)
    {
        stMoveParam->dir = 1;
        stMoveParam->pos = position;
        retVal = 1; 
    }
    else if(maxDistance<0)
    {
        endPos = 0 - position;

        if(endPos<=0 && endPos>=maxDistance) 
        {
            retVal = 1;
        } 
        else
        {
            retVal = 0; 
        }

        if(endPos > getELeCtlCurrPos(sid))
        {
            stMoveParam->dir = getELeCtlRetdir(sid);
            stMoveParam->pos = endPos - getELeCtlCurrPos(sid);
            // to original point
            //printf("set to ret dir,endPos[%d],currPos[%d]\n",endPos,uimCtlArr[sid-10].currPos);
        }
        else
        {
            /* code */
            //printf("set to ret dir,endPos[%d],currPos[%d]\n",endPos,uimCtlArr[sid-10].currPos);
            //leave original point
            stMoveParam->dir = 1 - getELeCtlRetdir(sid);
            stMoveParam->pos = getELeCtlCurrPos(sid) - endPos;
        }        
           
    }
    else
    {
        endPos = position;
        if(endPos>=0 && endPos<=maxDistance) 
        {
            retVal = 1;
        } 
        else
        {
            retVal = 0; 
        }

        if(endPos > getELeCtlCurrPos(sid))
        {
            //leave original point
            stMoveParam->dir = 1 - getELeCtlRetdir(sid);
            stMoveParam->pos = endPos - getELeCtlCurrPos(sid);
        }
        else
        {
            /* code */
            //to original point
            stMoveParam->dir = getELeCtlRetdir(sid);
            stMoveParam->pos = getELeCtlCurrPos(sid) - endPos;
        }
    }   
    
    printf("curPos[%d],posToMove[%d],posSet[%d],dir[%d],endPos[%d],maxDistance[%d]",
           getELeCtlCurrPos(sid),position,stMoveParam->pos,stMoveParam->dir,endPos,maxDistance);
    
    printf("valid[%d]\n",retVal);
    return retVal;
}

int CUim_MainCtl::getDev(unsigned int sidIn,uim_dev& devOut)
{
    int ret = -1;

    if(uimDevList.empty())
    {
        return ret;
    }
    list<uim_dev>::iterator itDev = uimDevList.begin();
    while(itDev!=uimDevList.end())
    {
        if(sidIn == itDev->sid)
        {
            devOut = *itDev;
            ret = 0;
            break;
        }
        itDev++;  
    }

    return ret;
}


void CUim_MainCtl::uim_query(query_param * pstQueryParam)
{
    uim_data dataRet;
    struct can_frame frameToSend;
    uim_frame dataRead;

    format_cmd(1,ENA,pstQueryParam->sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    if(pstQueryParam->queryCode == STP)
    {
        unsigned char data[4] = {0x00,0x00,0x00,0x00};
        format_cmd(1,STP,pstQueryParam->sid,4, data,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);
    }

    if(pstQueryParam->queryCode == POS)
    {
        //unsigned char data[4] = {0x00,0x00,0x00,0x00};
        unsigned char tempCmd = POS;
        if(getELeCtlLoopType(pstQueryParam->sid)==CLOSE_LOOP)
        {
            tempCmd = QEC;
        }
        format_cmd(1,tempCmd,pstQueryParam->sid,0, NULL,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);
    }
    
}

void CUim_MainCtl::initTecan(void)
{
   unsigned char buff[128];
   int len;

   memset(buff,0,128);
   pTecanInstance->setInitSOCAStr(0,buff,&len);

   //printf("in initTecan,len:%d\n",len);
   pTecanInstance->setCurrStatus(tecan_waiting_ack);
   int sendBytes = this->sendString(SID_TECAN,len,buff);
   //printf("in initTecan,sendbytes:%d\n",sendBytes);
   
   //memset(buff,0,128);

   //pTecanInstance->setInitAdp_Z_Str(buff,&len);
   //this->sendString(SID_TECAN,len,buff);

   pTecanInstance->startRecv();
}

extern CUim_MainCtl  * g_uimCanHandle;

void sendAckToGrpc(int statusCodeIn,int errorNumIn,int sid, int moduleId)
{
    msgQueueAck stMsgQueueAck;
    int msg_id_temp = 0;
    if(g_uimCanHandle)
    {
        memset(&stMsgQueueAck,0,sizeof(msgQueueAck));
        stMsgQueueAck.type = QUEUE_MSG_TYPE_GRPC;
        stMsgQueueAck.msg.statusCode = statusCodeIn;
        stMsgQueueAck.msg.sid = sid;
        stMsgQueueAck.msg.errorNum = errorNumIn;
        stMsgQueueAck.msg.moduleId = moduleId;

        // if(sid == 54)
        // {
        //     msg_id_temp = g_msg_id_Q1;
        // }
        // else
        // {
        //     msg_id_temp = g_uimCanHandle->getKeyId_FrameMQ();
        // }
        msg_id_temp = g_uimCanHandle->getKeyId_FrameMQ();        
        if(msgsnd(msg_id_temp, (void*)&stMsgQueueAck, sizeof(msgAck), 0) < 0)
        {
            printf("send ack failed\n");
        }

    }   
}

std::list<uim_sid_state> checkingRetList;
std::list<uim_sid_state> waitingRetList;

void procUimEleState(void)
{
    if(checkingRetList.empty())
    {
        std::list<uim_sid_state>::iterator itChecking = checkingRetList.begin();
        while(itChecking!=checkingRetList.end())
        {
            if(getELeCtlStatus(itChecking->sid) != CHECKING_RET)
            {
                std::list<uim_sid_state>::iterator itCheckingToDel = itChecking;
                itChecking++;
                checkingRetList.erase(itCheckingToDel);
                continue;
            }
            else
            {
                if(itChecking->waitTimeMs <= 0 )
                {
                    if(itChecking->max_cnt > 0)
                    {
                        itChecking->currInterv -= STATE_TIMEOUT_MS;
                        if(itChecking->currInterv <= 0)
                        {
                            uim_data dataRet;
                            struct can_frame frameToSend;
                            format_cmd(1,SFB,itChecking->sid,0, NULL,&dataRet);
                            encap_uim_data(&dataRet,&frameToSend);     
                            g_uimCanHandle->WriteCanPort(&frameToSend);

                            itChecking->currInterv = itChecking->waitInterv;
                            itChecking->max_cnt -= 1;
                        }
                        
                    }
                    else
                    {
                        std::list<uim_sid_state>::iterator itCheckingToDel = itChecking;
                        sendAckToGrpc(1,ACT_TIMEOUT_APP,itChecking->sid,-1);
                        itChecking++;
                        checkingRetList.erase(itCheckingToDel);
                        continue;
                    }
                }
                else
                {
                    itChecking->waitTimeMs -= STATE_TIMEOUT_MS;
                }
                
               itChecking++; 
            }
            
        }
    }
}